# import <needed_package_name>
import sys
import math # For Math functions
import sympy as sym # For declaring variables as symbols, make sure you have sympy installed in your system
from sympy import symbols
We have been given a seven DOF Kuka robot. We need to solve for the forward kinematics of the robot by applying the method given by Denavit-Hartenberg.
By following the rules layed out by the D-H convention of solving the problem
The co-ordinate frames are shown as below figure
We know that the general form of a transformation matrix is
xi = symbols('theta_i')
yi = symbols('alpha_i')
di = symbols('d_i')
ai = symbols('a_i')
cxi = sym.cos(xi)
sxi = sym.sin(xi)
cyi = sym.cos(yi)
syi = sym.sin(yi)
T0_i = sym.Matrix([[cxi, -sxi*cyi, sxi*syi, ai*cxi], [sxi, cxi*cyi, -cxi*syi, ai*syi], [0, syi, cyi, di], [0, 0, 0, 1]])
T0_i
x1 = symbols('x1') # Consider x1 as theta 1
d1 = symbols('d1')
c1 = sym.cos(x1)
s1 = sym.sin(x1)
T0_1 = sym.Matrix([[c1, 0, -s1, 0], [s1, 0, c1, 0], [0, -1, 0, d1], [0, 0, 0, 1]])
T0_1
#since the D-H parameter a is zero throughout, I don't mention it anywhere
x2 = symbols('x2') # Consider x2 as theta 2
d2 = symbols('d2') # d2 = 0
c2 = sym.cos(x2)
s2 = sym.sin(x2)
T1_2 = sym.Matrix([[c2, 0, s2, 0], [s2, 0, -c2, 0], [0, 1, 0, 0], [0, 0, 0, 1]])
T1_2
x3 = symbols('x3') # Consider x3 as theta 3
d3 = symbols('d3')
c3 = sym.cos(x3)
s3 = sym.sin(x3)
T2_3 = sym.Matrix([[c3, 0, s3, 0], [s3, 0, -c3, 0], [0, 1, 0, d3], [0, 0, 0, 1]])
T2_3
x4 = symbols('x4') # Consider x4 as theta 4
d4 = symbols('d4') # d4 = 0
c4 = sym.cos(x4)
s4 = sym.sin(x4)
T3_4 = sym.Matrix([[c4, 0, -s4, 0], [s4, 0, c4, 0], [0, -1, 0, 0], [0, 0, 0, 1]])
T3_4
x5 = symbols('x5') # Consider x5 as theta 5
d5 = symbols('d5')
c5 = sym.cos(x5)
s5 = sym.sin(x5)
T4_5 = sym.Matrix([[c5, 0, -s5, 0], [s5, 0, c5, 0], [0, -1, 0, d5], [0, 0, 0, 1]])
T4_5
x6 = symbols('x6') # Consider x6 as theta 6
d6 = symbols('d6') # d6 = 0
c6 = sym.cos(x6)
s6 = sym.sin(x6)
T5_6 = sym.Matrix([[c6, 0, s6, 0], [s6, 0, -c6, 0], [0, 1, 0, 0], [0, 0, 0, 1]])
T5_6
x7 = symbols('x7') # Consider x7 as theta 7
d7 = symbols('d7')
c7 = sym.cos(x7)
s7 = sym.sin(x7)
T6_7 = sym.Matrix([[c7, -s7, 0, 0], [s7, c7, 0, 0], [0, 0, 1, d7], [0, 0, 0, 1]])
T6_7
T0_7 = (T0_1)*(T1_2)*(T2_3)*(T3_4)*(T4_5)*(T5_6)*(T6_7)
T0_7
TF0_1 = T0_1.subs(x1,0)
TF1_2 = T1_2.subs(x2,0)
TF2_3 = T2_3.subs(x3,0)
TF3_4 = T3_4.subs(x4,0)
TF4_5 = T4_5.subs(x5,0)
TF5_6 = T5_6.subs(x6,0)
TF6_7 = T6_7.subs(x7,0)
TF1 = TF0_1*TF1_2*TF2_3*TF3_4*TF4_5*TF5_6*TF6_7
TF1
From the figure we can see that when all angles are 0 rad, the Pz co-ordinate is nothing but d1 + d3 + d5 + d7 And the orientation is a simple identity matrix (no rotation)
TF0_1 = T0_1.subs(x1,sym.pi/2)
TF1_2 = T1_2.subs(x2,0)
TF2_3 = T2_3.subs(x3,0)
TF3_4 = T3_4.subs(x4,0)
TF4_5 = T4_5.subs(x5,0)
TF5_6 = T5_6.subs(x6,0)
TF6_7 = T6_7.subs(x7,0)
TF2 = TF0_1*TF1_2*TF2_3*TF3_4*TF4_5*TF5_6*TF6_7
TF2
When the first angle (theta 1) is $\frac{\Pi}{2}$ rad and rest all are 0 rad, the rotation matrix is the orientation and the pz co-ordinate is nothing but d1 + d3 + d5 + d7
TF0_1 = T0_1.subs(x1,0)
TF1_2 = T1_2.subs(x2,0)
TF2_3 = T2_3.subs(x3,sym.pi/2)
TF3_4 = T3_4.subs(x4,0)
TF4_5 = T4_5.subs(x5,0)
TF5_6 = T5_6.subs(x6,0)
TF6_7 = T6_7.subs(x7,0)
TF3 = TF0_1*TF1_2*TF2_3*TF3_4*TF4_5*TF5_6*TF6_7
TF3
When the third angle (theta 3) is $\frac{\Pi}{2}$ rad and rest all are 0 rad, the rotation matrix is the orientation and the pz co-ordinate is nothing but d1 + d3 + d5 + d7
TF0_1 = T0_1.subs(x1,0)
TF1_2 = T1_2.subs(x2,0)
TF2_3 = T2_3.subs(x3,0)
TF3_4 = T3_4.subs(x4,sym.pi/2)
TF4_5 = T4_5.subs(x5,0)
TF5_6 = T5_6.subs(x6,0)
TF6_7 = T6_7.subs(x7,0)
TF4 = TF0_1*TF1_2*TF2_3*TF3_4*TF4_5*TF5_6*TF6_7
TF4
When the fourth angle (theta 4) is $\frac{\Pi}{2}$ rad and rest all are 0 rad, the rotation matrix is the orientation and the px co-ordinate is nothing but - d5 - d7 and the pz co-ordinate is d1 + d3
TF0_1 = T0_1.subs(x1,0)
TF1_2 = T1_2.subs(x2,0)
TF2_3 = T2_3.subs(x3,0)
TF3_4 = T3_4.subs(x4,0)
TF4_5 = T4_5.subs(x5,0)
TF5_6 = T5_6.subs(x6,sym.pi/2)
TF6_7 = T6_7.subs(x7,0)
TF5 = TF0_1*TF1_2*TF2_3*TF3_4*TF4_5*TF5_6*TF6_7
TF5
When the sixth angle (theta 6) is $\frac{\Pi}{2}$ rad and rest all are 0 rad, the rotation matrix is the orientation and the px co-ordinate is nothing but d7 and the pz co-ordinate is d1 + d3 + d5
We have with us a 6 DOF Puma robot we need to do Forward Velocity Kinematics and determine the Jacobian matrix for the robot using two methods Geometric Method and Derivative method
We use the rules set by D-H Method as below and assign co-ordinate frames
The co-ordinate frame assignment is shown below along with the D-H Parameters table
y1 = symbols('y1') # Consider y1 as theta 1
b1 = symbols('b1') # Using notation b instead of d
a1 = symbols('a1') # a1 = 0
ct1 = sym.cos(y1)
st1 = sym.sin(y1)
H0_1 = sym.Matrix([[ct1, 0, -st1, 0], [st1, 0, ct1, 0], [0, -1, 0, b1], [0, 0, 0, 1]])
H0_1
y2 = symbols('y2') # Consider y2 as theta 2
b2 = symbols('b2') # Using notation b instead of d, b2 (i.e d2) = 0
a2 = symbols('a2')
ct2 = sym.cos(y2)
st2 = sym.sin(y2)
H1_2 = sym.Matrix([[ct2, -st2, 0, a2*ct2], [st2, ct2, 0, a2*st2], [0, 0, 1, 0], [0, 0, 0, 1]])
H1_2
H0_2 = H0_1*H1_2
H0_2
y3 = symbols('y3') # Consider y3 as theta 3
b3 = symbols('b3') # Using notation b instead of d
a3 = symbols('a3')
ct3 = sym.cos(y3)
st3 = sym.sin(y3)
H2_3 = sym.Matrix([[ct3, 0, -st3, a3*ct3], [st3, 0, ct3, a3*st3], [0, -1, 0, b3], [0, 0, 0, 1]])
H2_3
H0_3 = H0_2*H2_3
H0_3
y4 = symbols('y4') # Consider y4 as theta 4
b4 = symbols('b4') # Using notation b instead of d
a4 = symbols('a4') #a4 = 0
ct4 = sym.cos(y4)
st4 = sym.sin(y4)
H3_4 = sym.Matrix([[ct4, 0, st4, 0], [st4, 0, -ct4, 0], [0, 1, 0, b4], [0, 0, 0, 1]])
H3_4
H0_4 = H0_3*H3_4
H0_4
y5 = symbols('y5') # Consider y5 as theta 5
b5 = symbols('b5') # Using notation b instead of d, b5 (i.e d5) = 0)
a5 = symbols('a5') # a5 = 0
ct5 = sym.cos(y5)
st5 = sym.sin(y5)
H4_5 = sym.Matrix([[ct5, 0, -st5, 0], [st5, 0, ct5, 0], [0, -1, 0, 0], [0, 0, 0, 1]])
H4_5
H0_5 = H0_4*H4_5
H0_5
y6 = symbols('y6') # Consider y1 as theta 1
b6 = symbols('b6') # Using notation b instead of d, b5 (i.e d5) = 0)
a6 = symbols('a6') # a6 = 0
ct6 = sym.cos(y6)
st6 = sym.sin(y6)
H5_6 = sym.Matrix([[ct6, 0, 0, 0], [st5, ct5, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
H5_6
H0_6 = H0_5*H5_6
H0_6
We know that for each Jacobian matrix for each transformation $Ji$ = $\binom{Jv}{Jw}$ = $\binom{Z(i-1) X (O(n) - O(i-1))}{Z(i-1)}$ (For a Revolute joint)
Where $Jv$ is the Jacobian for linear velocity and $Jw$ is the Jacobian for angular velocity
And put all individual jacobians together and form the final Jacobian Matrix from o to n $$
$$
On = sym.Matrix(H0_6.col(3)[0:3])
On
O0 = sym.Matrix([0, 0, 0])
O0
O1 = sym.Matrix(H0_1.col(3)[:3])
O1
O2 = sym.Matrix(H0_2.col(3)[:3])
O2
O3 = sym.Matrix(H0_3.col(3)[:3])
O3
O4 = sym.Matrix(H0_4.col(3)[:3])
O4
O5 = sym.Matrix(H0_5.col(3)[:3])
O5
r1 = On-O0
r1
r2 = On-O1
r2
r3 = On-O2
r3
r4 = On-O3
r4
r5 = On-O4
r5
r6 = On-O5
r6
z0 = sym.Matrix([0, 0, 1])
z0
z1 = sym.Matrix(H0_1.col(2)[:3])
z1
z2 = sym.Matrix(H0_2.col(2)[:3])
z2
z3 = sym.Matrix(H0_3.col(2)[:3])
z3
z4 = sym.Matrix(H0_4.col(2)[:3])
z4
z5 = sym.Matrix(H0_5.col(2)[:3])
z5
Now we compute the cross product of $Z(i-1)$ and $ri$ and complete the following Jacobian Matrix for each transformation $Ji$ = $\binom{Jv}{Jw}$ = $\binom{Z(i-1) X ri}{Z(i-1)}$
Now take all the $Ji$ column vectors and put them together as below to obtain the final Jacobian Matrix $J^{0}_6$ = $(J1, J2, J3, J4, J5, J6)$